##
# This class is used to give a simple PID controller instance by:
# 1. +PIDController.new()+ will generate a default PID controller
#    with default values (see +initialize+ method definition)
class PIDController
  attr_accessor :k_p, :k_i, :k_d

  # To get a new instance of PIDController,
  # use +PIDController.new()+.
  # The optional arguments are:
  # + +k_p+ for Proportional Part
  # + +k_i+ for Integral Part
  # + +k_d+ for Devirates Part
  def initialize(opts = {})
    @k_p = (opts[:k_p] || opts[:kp] || 0.0).to_f
    @k_i = (opts[:k_i] || opts[:ki] || 0.0).to_f
    @k_d = (opts[:k_d] || opts[:kd] || 0.0).to_f
    
    @err_integrate = 0 # used to calculate intgratal
    @err_previous  = 0 # used to calculate devirates
  end

  # Called to update the PID Controller
  # If passed in a block, it will yield the control value.
  # Otherwise, it will return the control value by default.
  # 
  # The +opts+ having the following avaliable args:
  # + +dt+ for time passed between each frame, default to +0.01+.
  # + +inverse+ for inverse the default way the +err+ is calculated.
  #   By default, +err = expect - actual+, with +inverse: true+
  #   will treat +err+ as +actual - expect+. 
  def update(err, opts = {}, &block)
    dt = (opts[:dt] || 0.01).to_f
    inverse = opts[:inverse] || false
    sign = inverse ? -1 : 1

    # update the status
    @err_integrate += err * dt
    err_derivate = (err - @err_previous) / dt
    @err_previous = err

    control_value = sign * (@k_p * err + @k_i * @err_integrate + @k_d * err_derivate)
    
    yield control_value if block
    
    return control_value
  end
end
